#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QPixmap>
#include "mat-image-tools.h"




MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    isopen=false;
    imgtype=COLOR;
    mycamera=nullptr;
    decX=0;
    decY=0;
}

MainWindow::~MainWindow()
{
    delete ui;
    if(mycamera==nullptr)
        return;
    if(mycamera->isRunning())
    {
        OpenNI::shutdown();
        mycamera->quit();
        mycamera->destroyed();
        mycamera=nullptr;
    }
}

void MainWindow::startcamera()
{
    if(mycamera == nullptr)
        mycamera=new camerathread(this);

    if(mycamera->isRunning())
        return;

    isopen=true;
    mycamera->start();
}

void MainWindow::stopcamera()
{
    isopen=false;
}

void MainWindow::depthimageactive()
{
    imgtype=DEPTH;
}

void MainWindow::colorimageactive()
{
    imgtype=COLOR;
}

void MainWindow::cameraprocess()
{
    auto version = openni::OpenNI::getVersion();
    std::cout << "OpenNI " << version.major << "." << version.minor << "." <<
                 version.maintenance << "." << version.build << std::endl;

    VideoFrameRef oniDepthImg, oniColorImg;

    OpenNI::shutdown();

    Status result = STATUS_OK;
    result = OpenNI::initialize();

    // open device
    Device device;
    result = device.open( openni::ANY_DEVICE );


    //// create color stream
    VideoStream oniColorStream;
    result = oniColorStream.create( device, openni::SENSOR_IR );
    //// create depth stream
    VideoStream oniDepthStream;
    result = oniDepthStream.create( device, openni::SENSOR_DEPTH );

    // set depth video mode
    VideoMode modeDepth;
    modeDepth.setResolution( 640, 480 );
    modeDepth.setFps( 30 );
    modeDepth.setPixelFormat( PIXEL_FORMAT_DEPTH_1_MM );
    oniDepthStream.setVideoMode(modeDepth);
    //oniDepthStream.setMirroringEnabled(true);
    //device.setImageRegistrationMode(ImageRegistrationMode::IMAGE_REGISTRATION_DEPTH_TO_COLOR);

    VideoMode modeColor;
    modeColor.setResolution( 640, 480 );
    modeColor.setFps( 30 );
    modeColor.setPixelFormat( PIXEL_FORMAT_RGB888 );
    oniColorStream.setVideoMode(modeColor);

    device.setImageRegistrationMode(ImageRegistrationMode::IMAGE_REGISTRATION_DEPTH_TO_COLOR);

    // start depth stream
    result = oniColorStream.start();
    result = oniDepthStream.start();
    while( isopen ) {

        if (oniColorStream.isValid() && imgtype == COLOR) {
            if ( oniColorStream.readFrame( &oniColorImg ) == STATUS_OK ) {
                cv::Mat cColorImg( oniColorImg.getHeight(), oniColorImg.getWidth(),
                                   CV_8UC3, (void*)oniColorImg.getData() );

                cv::cvtColor(cColorImg, cvimage, COLOR_BGR2GRAY); //this will put colors right
                //cv::imshow("Structured IR", frameColor);
                //moveWindow("Structured IR", oniColorImg.getWidth()+8, 0 );
            }
        }
        else if(oniDepthStream.isValid() && imgtype == DEPTH)
        {
            if ( oniDepthStream.readFrame( &oniDepthImg ) == STATUS_OK ) {
                cv::Mat cDepthImg( oniDepthImg.getHeight(), oniDepthImg.getWidth(),
                                   CV_16UC1, (void*)oniDepthImg.getData() );

                cv::Mat adjustedDepth = adjustDepth(cDepthImg);
                adjustedDepth.convertTo(cvimage, CV_8UC1, 255.0f/4000); //Darker
                //cv::imshow("Adjusted Depth", dispImage);
                //moveWindow("Adjusted Depth", 0, 0 );
            }
        }else
            {
            break;
        }
        QMetaObject::invokeMethod(this,"ondatachange");
        waitKey(10);
    }
    oniColorStream.destroy();
    oniDepthStream.destroy();
    device.close();
    OpenNI::shutdown();
    mycamera->quit();
    mycamera->destroyed();
    mycamera=nullptr;
}


void MainWindow::on_pushButton_clicked()
{
    startcamera();
}

void MainWindow::on_pushButton_2_clicked()
{
    stopcamera();
}

void MainWindow::on_pushButton_3_clicked()
{
    if(imgtype==DEPTH)
    {
        imgtype=COLOR;
    }else
        imgtype=DEPTH;
    ondatachange();
}

void MainWindow::ondatachange()
{
    if(cvimage.empty())
        return;
    QPixmap D;
    cv::Mat shiftx,shifty,result;
    if(decX<0)
    {
        shiftx=ShiftFrame(cvimage,std::abs(decX),shift_left);
    }else if(decX>0)
    {
        shiftx=ShiftFrame(cvimage,std::abs(decX),shift_right);
    }
    else
        shiftx=cvimage;
    if(decY<0)
    {
        shifty=ShiftFrame(shiftx,std::abs(decY),shift_up);
    }else if(decY>0)
    {
        shifty=ShiftFrame(shiftx,std::abs(decY),shift_down);
    }else
        shifty=shiftx;
    cvimage=shifty;

    cv::addWeighted(cvimage,0.5,shifty,0.5,0,result,-1);
    if(ui->checkBox_fit->isChecked())
    {
        D=Mat2QPixmapResized(result,ui->scrollArea->width(),ui->scrollArea->height(),true);
        ui->scrollArea->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        ui->scrollArea->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
    } else
    {
        D=Mat2QPixmap(result);
        ui->scrollArea->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOn);
        ui->scrollArea->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOn);
    }
    ui->label_view->setFixedWidth(D.width());
    ui->label_view->setFixedHeight(D.height());
    ui->scrollAreaWidgetContents->setFixedWidth(D.width());
    ui->scrollAreaWidgetContents->setFixedHeight(D.height());
    ui->label_view->setPixmap(D);
}


void MainWindow::on_checkBox_fit_clicked()
{
    ondatachange();
}
